#include <unistd.h>
#include <stdio.h>
#include <string.h>

#include "contr_suite.h"

int pinRotate = 2;
int minRotate = 540;
int cenRotate = 1470;
int maxRotate = 2400;
int maxRotateAngle = 90;
int minRotateAngle = -85;

int pinFront = 7;
int cenFront = 1510;
int maxFront = 150;

int pinCamPan = 3;
int cenCamPan = 1400;
int maxCamPan = 2400;
int minCamPan = 540;
int maxCamPanAngle = 89;
int minCamPanAngle = -82;

int pinCamTilt = 5;
int cenCamTilt = 1250;
int maxCamTilt = 1900;
int minCamTilt = 700;
int maxCamTiltAngle = 50;
int minCamTiltAngle = -60;

int map(int a1, int b1, int a2, int b2, int val) {
    return (val - a1) * (b2 - a2) / (b1 - a1) + a2;
}

int max(int a, int b) {
    return (a > b ? a : b);
}

int min(int a, int b) {
    return (a < b ? a : b);
}

int sign(int x) {
    return (x > 0 ? 1 : (x < 0 ? -1 : 0));
}

int main(void) {
    int modeling = 0;
    int fd = 1;

    if (!modeling)
        fd = openFile();

    for (;;) {
        char cmdLine[256], msg[256], cmd[256];
        cmdLine[0] = 0;
        if (!fgets(cmdLine, 256, stdin))
            break;
        strcpy(msg, cmdLine);
        cmd[0] = 0;
        sscanf(cmdLine, "%s", cmd);
        if (!strcmp(cmd, "rotate")) {
            int angle = 0;
            char keep[256];
            keep[0] = 0;
            sscanf(cmdLine, "%s%d%s", cmd, &angle, keep);
            if (keep[0] && strcmp(keep, "auto-reset")) {
                keep[0] = 0;
            }
            if (keep[0] != 0) {
                fprintf(stderr, "Command 'rotate' with auto-reset is not implemented yet. Use 'detach %d' to reset servo.\n", pinRotate);
            }
            angle = max(minRotateAngle, min(maxRotateAngle, angle));
            int interval;
            if (angle < 0)
                interval = map(minRotateAngle, 0, maxRotate, cenRotate, angle);
            else
                interval = map(0, maxRotateAngle, cenRotate, minRotate, angle);
            msg[sprintf(msg, "servo %d %d", pinRotate, interval)] = 0;
        } else if (!strcmp(cmd, "move")) {
            int speed = 50;
            sscanf(cmdLine, "%s%d", cmd, &speed);
            speed = max(-100, min(100, speed));
            if (speed == 0) {
                msg[sprintf(msg, "detach %d", pinFront)] = 0;
            } else {
                int interval;
                interval = cenFront + map(-100, 100, -maxFront, maxFront, speed);
                msg[sprintf(msg, "servo %d %d", pinFront, interval)] = 0;
            }
        } else if (!strcmp(cmd, "brake")) {
            int val = 1;
            sscanf(cmdLine, "%s%d", cmd, &val);
            val = !!val;
            if (val == 1)
                msg[sprintf(msg, "servo %d %d", pinFront, cenFront)] = 0;
            else
                msg[sprintf(msg, "detach %d", pinFront)] = 0;
        } else if (!strcmp(cmd, "cam-pan")) {
            int angle = 0;
            char keep[256];
            keep[0] = 0;
            sscanf(cmdLine, "%s%d%s", cmd, &angle, keep);
            if (keep[0] && strcmp(keep, "auto-reset")) {
                keep[0] = 0;
            }
            if (keep[0] != 0) {
                fprintf(stderr, "Command 'cam-pan' with auto-reset is not implemented yet. Use 'detach %d' to reset servo.\n", pinCamPan);
            }
            angle = max(minCamPanAngle, min(maxCamPanAngle, angle));
            int interval;
            if (angle < 0)
                interval = map(minCamPanAngle, 0, minCamPan, cenCamPan, angle);
            else
                interval = map(0, maxCamPanAngle, cenCamPan, maxCamPan, angle);
            msg[sprintf(msg, "servo %d %d", pinCamPan, interval)] = 0;
        } else if (!strcmp(cmd, "cam-tilt")) {
            int angle = 0;
            char keep[256];
            keep[0] = 0;
            sscanf(cmdLine, "%s%d%s", cmd, &angle, keep);
            if (keep[0] && strcmp(keep, "auto-reset")) {
                keep[0] = 0;
            }
            if (keep[0] != 0) {
                fprintf(stderr, "Command 'cam-tilt' with auto-reset is not implemented yet. Use 'detach %d' to reset servo.\n", pinCamTilt);
            }
            angle = max(minCamTiltAngle, min(maxCamTiltAngle, angle));
            int interval;
            if (angle < 0)
                interval = map(minCamTiltAngle, 0, maxCamTilt, cenCamTilt, angle);
            else
                interval = map(0, maxCamTiltAngle, cenCamTilt, minCamTilt, angle);
            msg[sprintf(msg, "servo %d %d", pinCamTilt, interval)] = 0;
        } else if (!strcmp(cmd, "quit")) {
            break;
        } else if (!strcmp(cmd, "delay")) {
            int millis = 1000;
            sscanf(cmdLine, "%s%d", cmd, &millis);
            if (millis > 0) {
                fprintf(stderr, "Command 'delay' is not implemented yet.\n");
            }
            msg[0] = 0;
        }
        if (msg[0] != 0) {
            // Have something to send
            sendMessage(msg, fd);
            if (!modeling)
                transferMessage(fd, 1);
            else
                printf("OK 1\n\n");
        }
    }
    if (!modeling)
        close(fd);
    return 0;
}
